//
// Created by engineai on 2024/07/03.
//

#ifndef ZQ_HUMANOID_SPINEBOARD_H
#define ZQ_HUMANOID_SPINEBOARD_H

#include "../cTypes.h"

/*!
 * Command to spine board
 */
struct SpiCommand
{
    float q_des_abad[4];
    float q_des_hip[4];
    float q_des_knee[4];

    float qd_des_abad[4];
    float qd_des_hip[4];
    float qd_des_knee[4];

    float kp_abad[4];
    float kp_hip[4];
    float kp_knee[4];

    float kd_abad[4];
    float kd_hip[4];
    float kd_knee[4];

    float tau_abad_ff[4];
    float tau_hip_ff[4];
    float tau_knee_ff[4];

    int32_t flags[4];
};

/*!
 * Data from spine board
 */
struct SpiData
{
    float q_abad[4];
    float q_hip[4];
    float q_knee[4];
    float qd_abad[4];
    float qd_hip[4];
    float qd_knee[4];
    float tau_abad[4];
    float tau_hip[4];
    float tau_knee[4];
    int32_t flags[4];
    int32_t spi_driver_status;
};

/*!
 * Spine board control logic
 */
class SpineBoard
{
public:
    SpineBoard() {}
    void init(float side_sign, s32 board);
    void run();
    void resetData();
    void resetCommand();
    SpiCommand *cmd = nullptr;
    SpiData *data = nullptr;
    float torque_out[3];

private:
    float side_sign;
    s32 board_num;

    const float max_torque[3] = {144.f, 144.f, 144.f}; // TODO CHECK WITH BEN
    const float wimp_torque[3] = {48.f, 48.f, 48.f};   // TODO CHECK WITH BEN
    const float disabled_torque[3] = {0.f, 0.f, 0.f};
    const float q_limit_p[3] = {1.5f, 5.0f, 0.f};
    const float q_limit_n[3] = {-1.5f, -5.0f, 0.f};
    const float kp_softstop = 100.f;
    const float kd_softstop = 0.4f;
    s32 iter_counter = 0;
};
#endif // ZQ_HUMANOID_SPINEBOARD_H
